#include <Servo.h>

/****************************/
/* servo parts              */
/****************************/
#define I_LEFT_FRONT_COXA 0
#define I_LEFT_FRONT_FEMUR 1
#define I_LEFT_FRONT_TIBIA 2
#define I_LEFT_MIDDLE_COXA 3
#define I_LEFT_MIDDLE_FEMUR 4
#define I_LEFT_MIDDLE_TIBIA 5
#define I_LEFT_BACK_COXA 6
#define I_LEFT_BACK_FEMUR 7
#define I_LEFT_BACK_TIBIA 8
#define I_RIGHT_FRONT_COXA 9
#define I_RIGHT_FRONT_FEMUR 10
#define I_RIGHT_FRONT_TIBIA 11
#define I_RIGHT_MIDDLE_COXA 12
#define I_RIGHT_MIDDLE_FEMUR 13
#define I_RIGHT_MIDDLE_TIBIA 14
#define I_RIGHT_BACK_COXA 15
#define I_RIGHT_BACK_FEMUR 16
#define I_RIGHT_BACK_TIBIA 17


#define N_SERVOS 18
Servo servos[N_SERVOS];
int servo_ind[N_SERVOS] = {
  I_LEFT_FRONT_COXA, I_LEFT_FRONT_FEMUR, I_LEFT_FRONT_TIBIA,
  I_LEFT_MIDDLE_COXA, I_LEFT_MIDDLE_FEMUR, I_LEFT_MIDDLE_TIBIA,
  I_LEFT_BACK_COXA, I_LEFT_BACK_FEMUR, I_LEFT_BACK_TIBIA,
  
  I_RIGHT_FRONT_COXA, I_RIGHT_FRONT_FEMUR, I_RIGHT_FRONT_TIBIA,
  I_RIGHT_MIDDLE_COXA, I_RIGHT_MIDDLE_FEMUR, I_RIGHT_MIDDLE_TIBIA,
  I_RIGHT_BACK_COXA, I_RIGHT_BACK_FEMUR, I_RIGHT_BACK_TIBIA,
};
int servo_pin[N_SERVOS] = {
  35, 36, 37,
  27, 28, 29,
  24, 25, 26,
  
  38, 39, 40,
  46, 47, 48,
  49, 50, 51
};
int servo_pos[N_SERVOS] = {
  90, 0, 0,
  90, 0, 0,
  90, 0, 0,
  
  90, 180, 180,
  90, 180, 180,
  90, 180, 180,
};
int servo_cal[N_SERVOS] = {
    0,  0,  0,
    0,  0,  0,
    0,  0,  0,
	
    0,  0,  0,
    0,  0,  0,
    0,  0,  0,
};
boolean servo_update[N_SERVOS] = {
    true,  true,  true,
    true,  true,  true,
    true,  true,  true,
	
    true,  true,  true,
    true,  true,  true,
    true,  true,  true,
};

boolean g_isReady = false;
boolean g_isConnected = false;
unsigned long g_time=0;
boolean g_isDebug = false;
void setup()
{
  Serial.begin(9600);
  Serial1.begin(9600);
  //Serial1.begin(115200);

  Serial.println("Initialize servomotors...");

  init_servos();
  setpackposition();
  update_servos();
  delay(5000);

  Serial.println("Wait commands...");
  g_isReady = true;
  
  g_time = millis();
}

void loop()
{
  unsigned long cur_time = millis();
  unsigned long slicetime = cur_time-g_time;
  g_time = cur_time;

  // got bluetooth command ?
  check_commands();
  
  // update servo positions
  update_servos();
  
  // send tick
  if(g_isConnected)
  {
    send_tick(slicetime);
  }
}

